
function dxx = Bcr4bp_EqmEMRot3D(t , xx , aux)
%
% BCR4BP动力学模型，仅状态，EMRot，3D
%
% 输入：
% t:            [1x1]            时间(s)
% xx:         [6x1]            状态(km , km/s)
% aux:       [1x1]            参数
%
% 输出：
% dxx:        [6x1]            动力学方程(km , km/s^2)
%
% 参考：
% [1] 2022_Boudad，附录推导
% [2] 2020_Pritchett, Page32, Equ 2.54-2.56
%
% 备注：
% 20240525，已检查
% 
% 时间：2024年06月03日
% 作者：张晨
% 单位：CSU
% 邮箱：chenzhang@csu.ac.cn
%%%%%%%%%%%%%%%%%%%%%%%

% ---------------- 载入参数 ----------------
mu = aux.EMRot.mu;
mus = aux.EMRot.mus;
as = aux.EMRot.as;
ws = aux.EMRot.ws;
l_star = aux.dim.EMRot_l;

% ---------------- 提取变量 ----------------
x = xx(1);
y = xx(2);
z = xx(3);
vx = xx(4);
vy = xx(5);
vz = xx(6);

% ---------------- 天体位置 ----------------
thetas = ws * t;

% B1位置
xb1 = 0;
yb1 = 0;
zb1 = 0;

% B2位置
xb2 = as * mus / (mus + 1) * cos(thetas);
yb2 = as * mus / (mus + 1) * sin(thetas);
zb2 = 0;

% 地球位置
xe = - mu;
ye = 0;
ze = 0;

% 月球位置
xm = 1 - mu;
ym = 0;
zm = 0;

% 太阳位置
xs = as * cos(thetas);
ys = as * sin(thetas);
zs = 0;

% % --------------- E/M 碰撞检测 + 最远地心距检测 ---------------
% earthRMin = 6378;
% moonRMin = 1738;
% earthRMax = 250e4;
% 
% cond1 = sqrt((x - xe)^2 + (y - ye)^2 + (z - ze)^2) < (earthRMin / l_star);
% cond2 = sqrt((x - xe)^2 + (y - ye)^2 + (z - ze)^2) > (earthRMax / l_star);
% cond3 = sqrt((x - xm)^2 + (y - ym)^2 + (z - zm)^2) < (moonRMin / l_star);
% 
% if any([cond1 , cond2 , cond3])
%     dxx = [];
%     return;
% end

% ----------------- EMrot动力学方程 -----------------
r_ec_3pow = ((x - xe)^2 + (y - ye)^2 + (z - ze)^2)^(1.5);
r_mc_3pow = ((x - xm)^2 + (y - ym)^2 + (z - zm)^2)^(1.5);
r_sc_3pow = ((x - xs)^2 + (y - ys)^2 + (z - zs)^2)^(1.5);

dxx = [vx;
    vy;
    vz;
    x + 2 * vy - (1 - mu) * (x - xe) / r_ec_3pow - mu * (x - xm) / r_mc_3pow - mus * (x - xs) / r_sc_3pow - mus / as^3 * xs;
    y - 2 * vx - (1 - mu) * (y - ye) / r_ec_3pow - mu * (y - ym) / r_mc_3pow - mus * (y - ys) / r_sc_3pow - mus / as^3 * ys;
    - (1 - mu) * (z - ze) / r_ec_3pow - mu * (z - zm) / r_mc_3pow - mus * (z - zs) / r_sc_3pow - mus / as^3 * zs];

end
